% Christian Hubicki

classdef ForceControllerClass < ControllerClass
    
    properties (Constant)
        Kp = 10000; %25
        Ki = 0; %10
        Kd = 0;
        
        controller_type = 'force_integral';
        
        LIMITS_ON = 0;
        
        torque_limit = 25; % 25 N-m
        
    end
    
    properties
        time_profile;
        force_profile;
        td_time;
        
        max_torque = 0;
        
    end
    
    methods
        
        function obj = ForceControllerClass
            
        end
        
        function u = flight(obj, t, x)
            u = [0; 0];
            %             u = [0; -20.0*x(ForceControlModelClass.doli)];% + -40*(x(ForceControlModelClass.oli)-360)];
        end
        
        function u = stance(obj, t, x, dxtdyt)
            Fs = ForceControlModelClass.kl * ( ForceControlModelClass.RZ(x) - ForceControlModelClass.R(x) );
            
            F_target = interp1(obj.time_profile, obj.force_profile, t - obj.td_time);
            
            if(isnan(F_target))
                F_target = 0;
            end
            
            u = [   0;
                obj.Kp * ( F_target - Fs )      ...
                + obj.Ki * ( x(ForceControlModelClass.fii ) )      ];
            
            %                 u = [   0;
            %                     interp1(obj.time_vector, obj.torque_vector, t - obj.td_time)];
            
            if(isnan(u(2)))
                u(2) = 0;
            end
            
            if(obj.LIMITS_ON && obj.torque_limit < abs(u(2)))
                u(2) = sign(u(2))*obj.torque_limit;
            end
            
            if(abs(u(2)) > abs(obj.max_torque))
                obj.max_torque = u(2);
            end
            
            %                 u = [   0;
            %                         interp1(obj.time_vector, obj.torque_vector, t - obj.td_time)];
            
            
        end
        
        function lo( obj, t, x )
            
        end
        
        function td( obj, t, x )
            obj.td_time = t;
        end
        
    end
    
end
